#include "CPG.h" 
float T=0.03;
float kb=5;
float km=5;

float sign(float x){
	float y;
	if(x>0)
		y=1;
	else if(x<0)
		y=-1;
	else
		y=0;
	return y;
}

void calCPG_db(float bo,float *dbo,float *dbt,float B){
	float b;
	b=*dbt;
	*dbt=*dbo+T*kb*(0.25*kb*(B-bo)-*dbo);
	*dbo=b;
}
void calCPG_b(float *bo,float dbt,float *bt){
	float b;
	b=*bt;
	*bt=*bo+T*dbt;
	*bo=b;
}

void calCPG_dm(float mo,float *dmo,float *dmt,float M){
	float m;
	m=*dmt;
	*dmt=*dmo+T*km*(0.25*km*(M-mo)-*dmo);
	*dmo=m;
}
void calCPG_m(float *mo,float dmt,float *mt){
	float m;
	m=*mt;
	*mt=*mo+T*dmt;
	*mo=m;
}

void calCPG_fi(float *fio,float *fit,float omiga,float R){
	float f;
	f=*fit;
	*fit=*fio+T*((1+R)*(1+R)-(R*R-1)*sign(sin(*fio)))*omiga/4/R;
	*fio=f;
}

float calCPG_angle(float b,float m,float fi){
	float angle;
	angle=b+m*sin(fi);
	if(abs(angle)<1)angle=0;
	return angle;
}
